#include "planner_core.h"
#include <boost/heap/binomial_heap.hpp>

namespace findPlanner {

struct CompareNodes{
  bool operator()(const mNode2d *lhs, const mNode2d *rhs) const{
    return lhs->getC() > rhs->getC();     // astar
    // return lhs->getG() > rhs->getG();    // djstar
  }
};

void Planner::initializeLookups(const cv::Mat &map)
{
  this->width = map.cols;
  this->height = map.rows;
  mCostMap = map;

  nodes2D = new mNode2d[width * height]();
  std::cout << "w_h: " << this->width << " " << this->height << std::endl;
}

bool Planner::astartfindPath(const std::pair<int, int> &start, const std::pair<int, int> &goal, std::vector<std::pair<int, int>> &plan)
{
  mNode2d start2d(start.first, start.second, 0, 0, nullptr);  // 起点
  mNode2d goal2d(goal.first, goal.second, 0, 0, nullptr);     // 终点
  boost::heap::binomial_heap<mNode2d *, boost::heap::compare<CompareNodes>> mOpen;  // 优先级队列
  start2d.updateH(goal2d);
  start2d.open();
  mOpen.push(&start2d);
  int iPred = start2d.setIdx(width);

  bool succcomplete = false;
  // mNode2d *nodes2D = new mNode2d[width * height]();

  for (int i = 0; i < width * height; ++i)
    nodes2D[i].reset();

  nodes2D[iPred] = start2d;
  while (!mOpen.empty())
  {
    mNode2d *nPred = mOpen.top();
    iPred = nPred->setIdx(width);

    if (nodes2D[iPred].isClosed()){
      mOpen.pop();
      continue;
    }
    else if (nodes2D[iPred].isOpen()){
      nodes2D[iPred].close();   // 闭区间
      mOpen.pop();

      if (*nPred == goal2d){
        // 匹配到了目标点
        while (nPred != nullptr){
          plan.emplace_back(std::make_pair(nPred->getX(), nPred->getY()));
          nPred = nPred->getPred();
        }
        succcomplete = true;
        std::cout << "find path: " << plan.size() << std::endl;
        break;
      }
      else{
        for (int i = 0; i < mNode2d::dir; i++)
        {
          mNode2d *nSucc = nPred->createSuccessor(i);
          int iSucc = nSucc->setIdx(width);

          // 邻居节点不能离开地图范围内、不能存在于闭区间、不能是障碍物点
          if (nSucc->isOnGrid(width, height) && !nodes2D[iSucc].isClosed() && static_cast<int>(mCostMap.at<uchar>(nSucc->getY(), nSucc->getX())) == 255)
          {
            nSucc->updateG();         // 更新邻居节点的成本代价值
            // int newG = nSucc->getG(); // 比较启发函数值
            // if (!nodes2D[iSucc].isOpen() || newG < nodes2D[iSucc].getG())

            int newC = nSucc->getC(); // 比较目标函数值
            if (!nodes2D[iSucc].isOpen() || newC < nodes2D[iSucc].getC())
            {
              nSucc->updateH(goal2d); // 更新邻居节点的启发函数值
              nSucc->open();          // 将邻居节点放入到开集合中
              nSucc->setPred(&nodes2D[iPred]);

              nodes2D[iSucc] = *nSucc;
              mOpen.push(&nodes2D[iSucc]);
              delete nSucc;
            }
            else{
              delete nSucc;
            }
          }
          else
            delete nSucc;
        }
      }
    }
    else{
      std::cout << "The situation does not exist." << std::endl;
    }
  }

  // delete[] nodes2D;
  return succcomplete;
}



bool Planner::findPath(const std::pair<int, int> &start, const std::pair<int, int> &goal, std::vector<std::pair<int, int>> &plan, bool check_cell)
{
  mNode2d start2d(start.first, start.second, 0, 0, nullptr);  // 起点
  mNode2d goal2d(goal.first, goal.second, 0, 0, nullptr);     // 终点
  boost::heap::binomial_heap<mNode2d *, boost::heap::compare<CompareNodes>> mOpen;  // 优先级队列
  start2d.updateH(goal2d);
  start2d.open();
  mOpen.push(&start2d);
  int iPred = start2d.setIdx(width);

  std::cout << "route—stage: " << check_cell << "\n";
  bool succcomplete = false;
  for (int i = 0; i < width * height; ++i)
    nodes2D[i].reset();

  nodes2D[iPred] = start2d;
  while (!mOpen.empty())
  {
    mNode2d *nPred = mOpen.top();
    iPred = nPred->setIdx(width);

    if (nodes2D[iPred].isClosed()){
      mOpen.pop();
      continue;
    }
    else if (nodes2D[iPred].isOpen()){
      nodes2D[iPred].close();   // 闭区间
      mOpen.pop();

      bool getGoal = false;
      if (!check_cell)
        getGoal = voronoiData.find(std::make_pair(nPred->getX(), nPred->getY())) != voronoiData.end(); // 找到的点为voronoi时即可停止
      else
        getGoal = (*nPred == goal2d);

      if (getGoal){
        // 匹配到了目标点
        while (nPred != nullptr){
          plan.emplace_back(std::make_pair(nPred->getX(), nPred->getY()));
          nPred = nPred->getPred();
        }
        succcomplete = true;
        std::reverse(plan.begin(), plan.end());
        std::cout << "find path: " << plan.size() << std::endl;
        break;
      }
      else{
        for (int i = 0; i < mNode2d::dir; i++)
        {
          mNode2d *nSucc = nPred->createSuccessor(i);
          int iSucc = nSucc->setIdx(width);

          bool flag = true;
          if (!voronoiData.empty() && check_cell)
            flag = voronoiData.find(std::make_pair(nSucc->getX(), nSucc->getY())) != voronoiData.end();

          // 邻居节点不能离开地图范围内、不能存在于闭区间、不能是障碍物点、必须在顶点集合中可以找到
          if (nSucc->isOnGrid(width, height) && !nodes2D[iSucc].isClosed() && static_cast<int>(mCostMap.at<uchar>(nSucc->getY(), nSucc->getX())) == 255 && flag)
          {
            nSucc->updateG();         // 更新邻居节点的成本代价值

            // int newG = nSucc->getG();
            // if (!nodes2D[iSucc].isOpen() || newG < nodes2D[iSucc].getG())

            int newC = nSucc->getC();
            if (!nodes2D[iSucc].isOpen() || newC < nodes2D[iSucc].getC()){
              nSucc->updateH(goal2d); // 更新邻居节点的启发函数值
              nSucc->open();          // 将邻居节点放入到开集合中
              // nSucc->setPred(&nodes2D[iPred]);

              nodes2D[iSucc] = *nSucc;
              mOpen.push(&nodes2D[iSucc]);
              delete nSucc;
            }
            else{
              delete nSucc;
            }
          }
          else
            delete nSucc;
        }
      }
    }
    else{
      std::cout << "The situation does not exist." << std::endl;
    }
  }

  return succcomplete;
}

} //namespace findPlanner















